/*
 * command.c
 *
 * Created: 27-02-2012 12:55:51
 *  Author: Bjarke
 */ 


#include "command.h"
#include "constants.h"
#include "protocol.h"
#include "../HAL/PWM.h"

#include <string.h>
#include <avr/io.h>

char LIGHT[MAX_ARRAY] = "Ligh";
char SPEED[MAX_ARRAY] = "Spee";

char NACK[MAX_ARRAY] = "NACK";
char ACK[MAX_ARRAY] = "ACK";

void packet2Command(char* packet, int lentgh)
{
	char commandType[MAX_ARRAY];
	char dataType[MAX_ARRAY];
	char dataPacket[MAX_ARRAY];
	char size = packet[1];
	
	switch(packet[0])
	{
		case 'C' :
			SendData('A',"ACK");
			
			memccpy(commandType,&packet[2],size-1,30);
			
			if(memcmp(commandType,LIGHT,size-1) == 0)
			{
				//pakcet[6] points to the variable sent with the LED packet
				//turnOnLED(packet[6]);
			}
			else if (memcmp(commandType,SPEED,size-3) == 0)
			{
				//pakcet[6] points to the first variable sent with the SPEED packet
				startRobot(packet[6],packet[7]);
				//turnOnLED(packet[7]);
			}
			
			/*
			commandType[size+1] = '\0';	
			SendArray(commandType,size);*/
		break;
		
		case 'D' :
			
			SendData('A',"ACK");
			
			memccpy(dataType,&packet[2],size-1,30);
			
			if(memcmp(dataType,LIGHT,size-1) == 0)
			{
				dataPacket[0] = (~PINA)+30;
				dataPacket[1] = '\0';
				SendData('L', dataPacket);
			}	
		break;
		
		case 'A' : 
				memccpy(dataType,&packet[2],size-1,30);
				if(memcmp(dataType,NACK,size-1) == 0)
				{
					SendData('L', dataPacket);
				}
				else if(memcmp(dataType,ACK,size-1) == 0)
				{
				}
				else
					SendData('A',"NACK");
				break;
	}	
	
	
}